/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.Joystick;

/**
 * Thread that performs the actions when any buttons are pressed.
 * @author 
 */
public class Team997ButtonThread extends Thread {

    private Team997Threading mRobot997;
    private Joystick mLeftStick;
    private Joystick mRightStick;
    private boolean mLifterRaised = false;
    private boolean mLifterExtended = false;
    private boolean mBallHolderOn = false;

    /**
     * The contructor.
     * @param A refernce to the 997 robot and its necessary objects.
     */
    public Team997ButtonThread(Team997Threading robot) {
        mRobot997 = robot;
        mLeftStick = mRobot997.getLeftStick();
        mRightStick = mRobot997.getRightStick();
    }

    public void run() {
        while (mRobot997.isOperatorControl() && mRobot997.isEnabled()) {
            if (mRightStick.getTrigger()) {
                performKick();
            }

            if (mRightStick.getRawButton(2)) {
                toggleBallHolder();

            }

            if (mLeftStick.getRawButton(2)) {
                raiseLifter();
            }

            if (mLeftStick.getRawButton(3)) {
                extendLifter();
            }

            if (mLeftStick.getRawButton(4)) {
                performLift();
            }
            Thread.yield();
            Timer.delay(0.0005);
        }
    }

    /**
     * Performs the steps in a kick.
     */
    private void performKick() {
        System.out.println("Kick");
    }

    /**
     * Toggles to ball holder on and off.
     */
    private void toggleBallHolder() {
        if (mBallHolderOn) {
            System.out.println("Ball Holder Off");
            mBallHolderOn = false;
        } else {
            System.out.println("Ball Holder On");
            mBallHolderOn = true;
        }
        while (mRightStick.getRawButton(1)) {
            Timer.delay(0.0005);
        }

    }

    /**
     * Raises the lifter.
     */
    private void raiseLifter() {
        mLifterRaised = true;
        System.out.println("Lifter Raised");
    }

    /*
     * Extends the lifter.
     */
    private void extendLifter() {
        if (mLifterRaised) {
            mLifterExtended = true;
            System.out.println("Lifter Extended");
        }

    }

    /**
     * Performs the lift.
     */
    private void performLift() {
        if (mLifterExtended) {
            System.out.println("Lift Performed");
        }
    }
}
